# 键盘控制
rosrun teleop_twist_keyboard teleop_twist_keyboard.py

# 路径规划测试
rosrun map_server map_server /home/xxty/trobot/trobot_ros1/maps/规划测试1/map.yaml
rosrun mc_ros test_path_generate

# 运行测试
# catkin_make --only-pkg-with-deps map_server
# catkin_make --only-pkg-with-deps amcl
# source ~/trobot/trobot_ros1/devel/setup.zsh

# 仿真运行测试
roslaunch trobot_stage turtlebot_stage.launch
roslaunch mc_ros mc_ros_simu.launch

# rosrun map_server map_server ~/trobot/trobot_ros1/config/simu_maps/lab_f_scan/lab_f_scan.yaml
# roslaunch trobot_stage amcl.launch
# roslaunch mc_ros test_motion_controller.launch



# sudo apt-get install ros-noetic-plotjuggler
# sudo apt-get install ros-noetic-plotjuggler-ros
# roscore
# rosrun plotjuggler plotjuggler 


# cartographer建图测试
roslaunch trobot_stage turtlebot_stage.launch
rosnode kill /map_server
rosnode kill /amcl
# 新建地图
roslaunch trobot_stage cartographer_trobot.launch

# 保存地图
# 结束轨迹
rosservice call /finish_trajectory 0
# 写入pb数据
mkdir -p /home/xxty/user_config/maps/cartographer/lab_f_scan
rosservice call /write_state "{filename: '/home/xxty/user_config/maps/cartographer/lab_f_scan/map.pbstream'}"
# 地图格式转换
rosrun cartographer_ros cartographer_pbstream_to_ros_map \
    -map_filestem=/home/xxty/user_config/maps/cartographer/lab_f_scan/map \
    -pbstream_filename=/home/xxty/user_config/maps/cartographer/lab_f_scan/map.pbstream \
    -resolution=0.05

# 地图加载
roslaunch trobot_stage cartographer_localization.launch

########################################
# 通用
########################################


########################################
# 仿真环境
########################################


########################################
# 真实环境
########################################
roslaunch robot_bringup trobot_orangepi.launch


roslaunch robot_bringup cartographer_trobot.launch

rosservice call /finish_trajectory 0
mkdir -p /home/xxty/user_config/maps/cartographer/room_20250320
rosservice call /write_state "{filename: '/home/xxty/user_config/maps/cartographer/room_20250320/map.pbstream'}"
rosrun cartographer_ros cartographer_pbstream_to_ros_map \
    -map_filestem=/home/xxty/user_config/maps/cartographer/room_20250320/map \
    -pbstream_filename=/home/xxty/user_config/maps/cartographer/room_20250320/map.pbstream \
    -resolution=0.05

roslaunch robot_bringup cartographer_localization.launch
rosrun cartographer_ros cartographer_occupancy_grid_node #注意 地图数据按频发布

rosbag record --lz4 /scan_raw /tf_static /imu /imu_raw /odom
rosbag record --lz4 /scan_raw /tf_static /imu /imu_raw /odom /map /scan_matched_points2 /tf

rosrun robot_bringup teleop_twist_keyboard.py
sudo systemctl start orangepi_startup.service